# -*- encoding: UTF-8 -*-

import math
import thread
import threading
from naoqi import ALModule
from naoqi import ALBroker
from naoqi import ALProxy
import sys
import os
import time
import cv2
import numpy as np

NAO_IP = "192.168.1.103"
NAO_PORT = 9559
myBroker1 = ALBroker("myBroker",
                     "0.0.0.0",  # listen to anyone
                     0,  # find a free port and use it
                     NAO_IP,  # parent broker IP
                     NAO_PORT)
sonarproxy = ALProxy("ALSonar")
sonarproxy.subscribe("myApplication")
memorypxy = ALProxy("ALMemory")
alproxy = ALProxy("ALMotion")
cam = ALProxy("ALVideoDevice")
tts = ALProxy("ALTextToSpeech")
posture = ALProxy("ALRobotPosture")
obstacle_R = 0.26  # 障碍物半径 0.25
length = 0.26  # 机器人半径 0.24
finaldis = 1.0
MINdis = 0.7
leftdis = 8
rightdis = 8
mindis = 0.45
mistake_turnstra = 0.2
mistake_turn1 = 0.2
mistake_turn2 = 0.2
speed_x = 0.07
speed_y = 0.0
speed_z = 0.0
angleleft = 0.0
angleright = 0.0
# finalcoor=(0.0,0.0)
# l=threading.RLock()
flag = 1
sonarflag = 0
initangle = 0.0

class mythread(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name

    def run(self):
        while True:
            global speed_x, leftdis, rightdis, fixangle, flag, sonarflag
            leftdis, rightdis = sonardetect()
            print "%f,%f" % (leftdis, rightdis)
            if (min(leftdis, rightdis) < mindis):
                setRoute(min(leftdis, rightdis) - 0.1)
                sonarproxy.unsubscribe("myApplication")
                break
            time.sleep(0.8)

class runthread(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name

    def run(self):
        while True:
            # global speed_x,speed_y,speed_z
            alproxy.move(speed_x, speed_y, speed_z)
            if (flag != 1):
                alproxy.stopMove()
                break
            time.sleep(0.2)
global x,run
x = mythread("A")
run = runthread("B")
def sonardetect():
    ldis = memorypxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
    rdis = memorypxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
    sldis = str(ldis)
    srdis = str(rdis)
    sldis = sldis[0:4]
    srdis = srdis[0:4]

    sldis = float(sldis)
    srdis = float(srdis)

    LEFTDISTANCE = sldis
    RIGHTDISTANCE = srdis
    return (LEFTDISTANCE, RIGHTDISTANCE)

def processdata(data):
    # print initangle
    if (data >= 0 and initangle <= 0):
        if (math.pi + initangle >= data):
            return -(data - initangle)
        else:
            return 2 * math.pi + initangle - data
    elif (data <= 0 and initangle >= 0):
        if (math.pi - initangle >= -data):
            return initangle - data
        else:
            return -(2 * math.pi - initangle + data)
    else:
        return initangle - data

def setRoute(dis):
    r=0.5*math.pow(dis+obstacle_R,2)/(obstacle_R+length)-(obstacle_R+length)*0.5
    turn_degree = math.atan((obstacle_R+dis)/r)
    #X = (obstacle_R + length) * math.sin(turn_degree)
    speed = 0.08
    '''
    leftArmEnable = False
    rightArmEnable = False
    alproxy.setMoveArmsEnabled(leftArmEnable, rightArmEnable)
    '''
    global speed_x, speed_y, speed_z, flag, initangle
    '''
     z = -(math.pi-2*math.atan(dis/bottom))
     x = dis / math.sin(-z)
     y=0'''

    speed_x = speed
    fixangle = -processdata(memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
    t1 = r * math.fabs(-turn_degree+fixangle) / speed+2.0
    speed_z = (-turn_degree + fixangle) / t1
    speed_y = 0
    time.sleep(t1)
    '''
    speed_y = y/t1
    speed_x=x/t1
    speed_z=z/t1
    '''

    '''
    t2=10#3.5
    speed_y = 0
    if (dis - X > 0.02):
        speed_x=2.3*z*X/math.sin(z)/t2
    else:
        speed_x=2.3*(X+bottom/math.tan(-z)-car_r*math.cos(z))*z/math.sin(z)/t2
    #print X,speed_x
    speed_z=-2*z/t2
    time.sleep(t2)'''
    fixangle = -turn_degree - processdata(memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
    speed_x = speed
    t2 = (obstacle_R+length)*math.fabs(2*turn_degree+fixangle) / speed
    speed_y = 0
    speed_z = (fixangle+2*turn_degree) / t2
    time.sleep(t2)

    '''
    fixangle = -processdata(memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
    #t4=3
    speed_x=speed
    if (math.fabs(fixangle) > 0.08):
        t4 = (finaldis+obstacle_R-X) * fixangle / math.sin(fixangle) / speed_x
    else:
        t23 = (finaldis+obstacle_R-X) / speed_x
    speed_y=0
    speed_z=fixangle/t4
    time.sleep(t4+1)'''
    # flag=0

    '''
    t3=10
    if (dis - X > 0.02):
        speed_x = x / t3
    else:
        speed_x = car_r*(1+math.cos(z))/math.sin(-z)/t3
    speed_z = z / t3
    speed_y = 0.0
    time.sleep(t3)  # 4'''

    speed_x = speed
    fixangle = turn_degree-processdata(memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value"))
    t3 = r * math.fabs(-turn_degree+fixangle) / speed+2.0
    speed_z = (-turn_degree + fixangle) / t3
    speed_y = 0
    time.sleep(t3+2)

    flag = 1

# thread
event = threading.Event()

# vision
'''
resolution = 3  # Image of 1280*960px
colorSpace = 13
fps = 30  # Supported Framerate: from 1 to 30 fps
string=time.strftime('%X')
nameId = cam.subscribeCamera(string, 0, resolution, colorSpace, fps)'''
# Global variable to store the ReactToTouch module instance
ReactToTouch = None
memory = None

class ReactToTouch(ALModule):
    """ A simple module able to react
        to touch events.
    """

    def __init__(self, name):
        ALModule.__init__(self, name)
        # No need for IP and port here because
        # we have our Python broker connected to NAOqi broker

        # Create a proxy to ALTextToSpeech for later use
        self.tts = ALProxy("ALTextToSpeech")
        self.move = ALProxy("ALMotion")
        self.tts.say("start")
        # Subscribe to TouchChanged event:
        global memory
        memory = ALProxy("ALMemory")
        memory.subscribeToEvent("TouchChanged",
                                "ReactToTouch",
                                "onTouched")

    def onTouched(self, strVarName, value):
        """ This will be called each time a touch
        is detected.

        """
        # Unsubscribe to the event when talking,
        # to avoid repetitions
        memory.unsubscribeToEvent("TouchChanged",
                                  "ReactToTouch")

        touched_bodies = []
        for p in value:
            if p[1]:
                print p[0]
                touched_bodies.append(p[0])
                if p[0] == "Head/Touch/Middle":
                    #setFinger()
                    pass
                if p[0] == "Head/Touch/Front":
                    dete()
        # self.say(touched_bodies)
        # Subscribe again to the event
        memory.subscribeToEvent("TouchChanged",
                                "ReactToTouch",
                                "onTouched")

def main_():
    global ReactToTouch
    ReactToTouch = ReactToTouch("ReactToTouch")
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print
        print "Interrupted by user, shutting down"
        myBroker1.shutdown()
        sys.exit(0)

def main():

    alproxy.move(0.05, 0.0, 0.0)
    # direction=getdirection()
    # alproxy.moveTo(0.0,0.0,direction)
    # 0.464350288425 0.390809439382
    # 0.464350288425 0.0876114540325

    # 0.464350288425 1.56323775753
    # 0.464350288425 0.35044581613

def dete():
    global  x,run
    global rightdis, leftdis, flag, initangle

    # pic = vision.getimg(NAO_IP, NAO_PORT)
    # ge = threading.Thread(target=geterror, args=(pic,))
    # ge.start()
    # time.sleep(1.0)
    initangle = memorypxy.getData("Device/SubDeviceList/InertialSensor/AngleZ/Sensor/Value")
    x.start()
    run.start()
    '''
    while True:
        #print ("??%s" % leftdis)
        #print ("??%s" % rightdis)
        if (min(rightdis, leftdis) <= mindis):
            #tts.say("检测到左方障碍物")
            print "@#"
            #setRoute(min(rightdis, leftdis))
            break
        #time.sleep(0.2)
    #flag = 0
    '''

#if __name__ == "__main__":
    # main()
    # geterror(vision.getimg(NAO_IP, NAO_PORT))
    # run.start()
    # x.start()
    #_main()
    #main_()
    # setRoute(0.66)

